import math


def quaternion_to_euler(zxy_quaternion):
    q_w, q_x, q_y, q_z = zxy_quaternion

    # 计算俯仰角 θ
    sin_theta = -2 * (q_x * q_z - q_w * q_y)
    if abs(sin_theta) >= 1:
        # 处理万向节锁情况（θ = ±π/2）
        theta = math.copysign(math.pi / 2, sin_theta)
    else:
        theta = math.asin(sin_theta)

    cos_theta = math.cos(theta)
    if cos_theta != 0:
        # 计算航向角 ψ 和滚转角 φ
        psi = math.atan2(2 * (q_w * q_x + q_y * q_z), 1 - 2 * (q_x ** 2 + q_y ** 2))
        phi = math.atan2(2 * (q_w * q_z + q_x * q_y), 1 - 2 * (q_y ** 2 + q_z ** 2))
    else:
        # 万向节锁时，ψ 设为0，φ 直接计算
        psi = 0.0
        phi = math.atan2(2 * (q_w * q_y - q_x * q_z), 1 - 2 * (q_y ** 2 + q_z ** 2))

    return {
        "yaw_psi": psi,  # 航向角（Z轴）
        "pitch_theta": theta,  # 俯仰角（X轴）
        "roll_phi": phi  # 滚转角（Y轴）
    }